package FRC2856.Robot;

import edu.wpi.first.wpilibj.ADXL345_I2C;
import edu.wpi.first.wpilibj.ADXL345_I2C.AllAxes;
import edu.wpi.first.wpilibj.CounterBase.EncodingType;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.SpeedController;

public class DriveTrain{
    
    private double Kp,Ki,Kd;
    public ADXL345_I2C accelerometer;
    public Encoder leftEnc,rightEnc;
    public Gyro gyro;
    public PIDController leftPID,rightPID;
    public RobotDrive m_robDrive;
    public SpeedController leftMotor,rightMotor;
    
    public DriveTrain(){
        Kp = 2.0;
        Ki = 0.2;
        Kd = 2.0;
        accelerometer = new ADXL345_I2C(1, ADXL345_I2C.DataFormat_Range.k4G);
        leftEnc = new Encoder(RobotMap.dtLeftEncoderModule,RobotMap.dtLeftEncoderChannelA,RobotMap.dtLeftEncoderModule,RobotMap.dtLeftEncoderChannelB,RobotMap.dtLeftEncoderDirection,EncodingType.k4X);
        leftEnc.setDistancePerPulse(0.002182);  // Analog bot = 0.00636
        leftEnc.start();
        rightEnc = new Encoder(RobotMap.dtRightEncoderModule,RobotMap.dtRightEncoderChannelA,RobotMap.dtRightEncoderModule,RobotMap.dtRightEncoderChannelB,RobotMap.dtRightEncoderDirection,EncodingType.k4X);
        rightEnc.setDistancePerPulse(0.002182);  // Analog bot = 0.00636
        rightEnc.start();
        gyro = new Gyro(RobotMap.gyroChannel);
        leftMotor = new MotorDrive(RobotMap.dtLeftMotorModule, RobotMap.dtLeftMotorChannel);
        rightMotor = new Jaguar(RobotMap.dtRightMotorModule, RobotMap.dtRightMotorChannel);
        m_robDrive = new RobotDrive(leftMotor,rightMotor);
        
        leftEnc.setPIDSourceParameter(Encoder.PIDSourceParameter.kDistance);
        rightEnc.setPIDSourceParameter(Encoder.PIDSourceParameter.kDistance);
        leftPID = new PIDController(Kp, Ki, Kd, leftEnc, leftMotor, PIDController.kDefaultPeriod);
        rightPID = new PIDController(Kp, Ki, Kd, rightEnc, rightMotor, PIDController.kDefaultPeriod);
        leftPID.setContinuous();
        rightPID.setContinuous();
        leftPID.setOutputRange(-0.33, 0.33);
        rightPID.setOutputRange(-0.33, 0.33);
    }

    public void arcadeDrive(GenericHID l){
        if( !(leftPID.isEnable() || rightPID.isEnable()) ){
            m_robDrive.arcadeDrive(l);
        }
    }

    public void autoBalance(){
        
    }

    public double[] decrementP(){
        double p,i,d;
        double[] returnValue = new double[2];
        p = leftPID.getP();
        i = leftPID.getI();
        d = leftPID.getD();
        p -= 0.1;
        leftPID.setPID(p, i, d);
        returnValue[0] = p;
        p = rightPID.getP();
        i = rightPID.getI();
        d = rightPID.getD();
        p -= 0.1;
        rightPID.setPID(p, i, d);
        returnValue[1] = p;
        return returnValue;
    }

    public double[] decrementI(){
        double p,i,d;
        double[] returnValue = new double[2];
        p = leftPID.getP();
        i = leftPID.getI();
        d = leftPID.getD();
        i -= 0.01;
        leftPID.setPID(p, i, d);
        returnValue[0] = i;
        p = rightPID.getP();
        i = rightPID.getI();
        d = rightPID.getD();
        i -= 0.01;
        rightPID.setPID(p, i, d);
        returnValue[1] = i;
        return returnValue;
    }

    public double[] decrementD(){
        double p,i,d;
        double[] returnValue = new double[2];
        p = leftPID.getP();
        i = leftPID.getI();
        d = leftPID.getD();
        d -= 0.1;
        leftPID.setPID(p, i, d);
        returnValue[0] = d;
        p = rightPID.getP();
        i = rightPID.getI();
        d = rightPID.getD();
        d -= 0.1;
        rightPID.setPID(p, i, d);
        returnValue[1] = d;
        return returnValue;
    }

    public void disable(){
        moveStop();
    }

   public double[] getAccelerations(){
       AllAxes ac = accelerometer.getAccelerations();
       double[] returnValue = new double[3];
       returnValue[0] = ac.XAxis;
       returnValue[1] = ac.YAxis;
       returnValue[2] = ac.ZAxis;
       return returnValue;
   }

   public double getGyro(){
       return gyro.getAngle();
   }

    public double[] getEncoderDist(){
        double[] returnValue = new double[2];
        returnValue[0] = leftEnc.getDistance();
        returnValue[1] = rightEnc.getDistance();
        return returnValue;
    }

    public double[] getEncoderRate(){
        double[] returnValue = new double[2];
        returnValue[0] = leftEnc.getRate();
        returnValue[1] = rightEnc.getRate();
        return returnValue;
    }

    public double[] getPID(){
        double[] returnValue = new double[2];
        returnValue[0] = leftPID.get();
        returnValue[1] = rightPID.get();
        return returnValue;
    }

    public double[] incrementP(){
        double p,i,d;
        double[] returnValue = new double[2];
        p = leftPID.getP();
        i = leftPID.getI();
        d = leftPID.getD();
        p += 0.1;
        leftPID.setPID(p, i, d);
        returnValue[0] = p;
        p = rightPID.getP();
        i = rightPID.getI();
        d = rightPID.getD();
        p += 0.1;
        rightPID.setPID(p, i, d);
        returnValue[1] = p;
        return returnValue;
    }

   public double[] incrementI(){
        double p,i,d;
        double[] returnValue = new double[2];
        p = leftPID.getP();
        i = leftPID.getI();
        d = leftPID.getD();
        i += 0.01;
        leftPID.setPID(p, i, d);
        returnValue[0] = i;
        p = rightPID.getP();
        i = rightPID.getI();
        d = rightPID.getD();
        i += 0.01;
        rightPID.setPID(p, i, d);
        returnValue[1] = i;
        return returnValue;
    }

   public double[] incrementD(){
        double p,i,d;
        double[] returnValue = new double[2];
        p = leftPID.getP();
        i = leftPID.getI();
        d = leftPID.getD();
        d += 0.1;
        leftPID.setPID(p, i, d);
        returnValue[0] = d;
        p = rightPID.getP();
        i = rightPID.getI();
        d = rightPID.getD();
        d += 0.1;
        rightPID.setPID(p, i, d);
        returnValue[1] = d;
        return returnValue;
    }

    class MotorDrive extends Jaguar {
        public MotorDrive(int module, int port) {
            super(module, port);
        }
        //Calls the super set method and gives it the negated speed.
        public void set(double speed) {
            super.set(-speed);
        }
    }

    public void moveDist(double left, double right){
        leftPID.reset();
        rightPID.reset();
        leftEnc.reset();
        rightEnc.reset();
        leftPID.setSetpoint(left);
        rightPID.setSetpoint(right);
        leftPID.enable();
        rightPID.enable();
        m_robDrive.setSafetyEnabled(false);  // disable user watchdog
    }

    public void moveDistUpdate(double left, double right){
        if(leftPID.isEnable() || rightPID.isEnable()){
            // PID is enabled, update the values
            leftPID.setSetpoint(left);
            rightPID.setSetpoint(right);            
        }
        else{
            // PID is not enabled, call moveDist
            moveDist(left, right);
        }
    }

    public void moveEffort(double left, double right){
        m_robDrive.setLeftRightMotorOutputs(left,right);
    }

    public void moveStop(){
        leftPID.disable();
        rightPID.disable();
        moveEffort(0,0);
        //pidEnabled = false;
        m_robDrive.setSafetyEnabled(true); // enable user watchdog
    }

    public boolean pidStatus(){
        return (leftPID.isEnable() || rightPID.isEnable());
    }

   public void resetGyro(){
       gyro.reset();
   }

    public void setOutputRange(double range){
        double rangeMin = (-1)*range;
        leftPID.setOutputRange(rangeMin, range);
        rightPID.setOutputRange(rangeMin, range);
    }

    public void tankDrive(GenericHID l, GenericHID r){
        if( !(leftPID.isEnable() || rightPID.isEnable()) ){
            m_robDrive.tankDrive(l,r);
        }
    }

}
